#include <jni.h>
#include <string>
#include <cmath>
#include "KFApp.h"

static CQEAHRS *AHRSApp;
static CKFApp *kf;

/*******************************************************
 * AHRS 初始化函数
*******************************************************/
extern "C"
JNIEXPORT void JNICALL
Java_com_tmrnavi_myapplication_MainActivity_AHRS_1Init(JNIEnv *env, jobject thiz, jdouble ts) {
    // TODO: implement AHRS_Init()
    AHRSApp = new CQEAHRS(TS);
    kf = new CKFApp(TS);
}

/*******************************************************
 * AHRS 更新函数
*******************************************************/
extern "C"
JNIEXPORT jfloatArray JNICALL
Java_com_tmrnavi_myapplication_MainActivity_AHRS_1Update(JNIEnv *env, jobject thiz, jfloatArray wib0, jfloatArray fsf0, jfloatArray mag0) {
    // TODO: implement AHRS_Update()

    float att[3] = {0.0};
    float wib[3], fsf[3], mnb[3];
    CVect3 gyro, acc, mag;

    env->GetFloatArrayRegion(wib0,0,3, wib);
    env->GetFloatArrayRegion(fsf0,0,3, fsf);
    env->GetFloatArrayRegion(mag0,0,3, mnb);
    gyro = CVect3(wib);
    acc  = CVect3(fsf);
    mag  = CVect3(mnb);
    //acc /= norm(acc);
    mag /= norm(mag);

    AHRSApp->Update(gyro, acc, mag, TS);
    att[0] = AHRSApp->att.i/DEG;
    att[1] = AHRSApp->att.j/DEG;
    att[2] = AHRSApp->att.k/DEG;

    jfloatArray attArray = env->NewFloatArray(3);
    env->SetFloatArrayRegion(attArray,0,3, att);
    return attArray;
}


extern "C"
JNIEXPORT void JNICALL
Java_com_tmrnavi_myapplication_MainActivity_KFAPP_1Init(JNIEnv *env, jobject thiz, jdoubleArray vn0, jdoubleArray pos0) {
    // TODO: implement KFAPP_Init()
    double veln0[3], posi0[3];
    env->GetDoubleArrayRegion(vn0,0,3,veln0);
    env->GetDoubleArrayRegion(pos0,0,3,posi0);
    kf->Init(CSINS(AHRSApp->att, CVect3(veln0), CVect3(posi0)));
}

extern "C"
JNIEXPORT jdoubleArray JNICALL
Java_com_tmrnavi_myapplication_MainActivity_KFAPP_1TUpdate(JNIEnv *env, jobject thiz,
                                                          jfloatArray wib0, jfloatArray fsf0) {
    // TODO: implement KFAPP_Update()
    double avpxp[42] = {0.0};
    float wib[3], fsf[3];
    CVect3 wm, vm;

    env->GetFloatArrayRegion(wib0,0,3, wib);
    env->GetFloatArrayRegion(fsf0,0,3, fsf);

    wm = CVect3(wib)*(glv.dps*TS);
    vm = CVect3(fsf)*TS;
    kf->Update(&wm, &vm, 1, TS);

    avpxp[0] = kf->sins.att.i/DEG; avpxp[1] = kf->sins.att.j/DEG; avpxp[2] = kf->sins.att.k/DEG;
    avpxp[3] = kf->sins.vn.i;      avpxp[4] = kf->sins.vn.j;       avpxp[5] = kf->sins.vn.k;
    avpxp[6] = kf->sins.pos.i;     avpxp[7] = kf->sins.pos.j;      avpxp[8] = kf->sins.pos.k;
    for (int i = 0; i < 16; i++) {
        avpxp[i+9] = kf->Xk.dd[i];
        avpxp[i+25] = sqrt(kf->Pk(i,i));
    }
    avpxp[41] = kf->sins.tk;
    jdoubleArray avpArray = env->NewDoubleArray(42);
    env->SetDoubleArrayRegion(avpArray,0,42, avpxp);
    return avpArray;
}

extern "C"
JNIEXPORT jdoubleArray JNICALL
Java_com_tmrnavi_myapplication_MainActivity_KFAPP_1MUpdate(JNIEnv *env, jobject thiz, jdoubleArray gnssdata0) {
    // TODO: implement KFAPP_MUpdate()
    double gnssdata[9];
    CVect3 gnssp, gnssv;
    static double xkpk[33] = {0.0};
    env->GetDoubleArrayRegion(gnssdata0,0,9,gnssdata);

    gnssp = CVect3(&gnssdata[2]);
    gnssv = CVect3(&gnssdata[5]);
    for (int i = 0; i < 9; i++)
        xkpk[i] = kf->Xk.dd[i];
    for (int i = 9; i < 15; i++)
        xkpk[i] += kf->Xk.dd[i];
    xkpk[15] = kf->Xk.dd[15];
    for (int i = 0; i < 16; i++) {
        xkpk[i+16] = sqrt(kf->Pk(i,i));
    }
    xkpk[32] = gnssdata[1];

    if (gnssdata[0] <= 1.5)
        kf->SetMeas(gnssp);
    else
        kf->SetMeas(gnssp, gnssv);      // 全部使用速度量测

    jdoubleArray xkpkArray = env->NewDoubleArray(33);
    env->SetDoubleArrayRegion(xkpkArray,0,33, xkpk);
    return xkpkArray;
}
extern "C"
JNIEXPORT jdoubleArray JNICALL
Java_com_tmrnavi_myapplication_MainActivity_KFAPP_1SetParas(JNIEnv *env, jobject thiz,
                                                            jfloatArray kf_paras) {
    float kfParas[27];
    env->GetFloatArrayRegion(kf_paras,0,27, kfParas);
    CGLV glv;
    kfParas[0] *= glv.deg;     kfParas[1] *= glv.deg;     kfParas[2] *= glv.deg;
    kfParas[6] /= glv.Re;      kfParas[7] /= glv.Re;
    kfParas[9] *= glv.dph;     kfParas[10] *= glv.dph;    kfParas[11] *= glv.dph;
    kfParas[12] *= glv.mg;     kfParas[13] *= glv.mg;     kfParas[14] *= glv.mg;
    kfParas[15] *= glv.dpsh;   kfParas[16] *= glv.dpsh;   kfParas[17] *= glv.dpsh;
    kfParas[18] *= 1000*glv.ugpsHz; kfParas[19] *= 1000*glv.ugpsHz; kfParas[20] *= 1000*glv.ugpsHz;
    kfParas[24] /= glv.Re;     kfParas[25] /= glv.Re;
    for (int i = 0; i < 15; i++) {
        kf->Pk(i,i) = kfParas[i]*kfParas[i];
    }
    for (int i = 0; i < 6; i++) {
        kf->Qt(i) = kfParas[i+15]*kfParas[i+15]*TS;
        kf->Rt(i) = kfParas[i+21]*kfParas[i+15];
    }
}